package OperatorStation;

import se.lth.control.DoublePoint;
import se.lth.control.realtime.AnalogIn;
import se.lth.control.realtime.AnalogOut;
import se.lth.control.realtime.*;
//import se.lth.control.realtime.IOChannelException;
import se.lth.control.realtime.Semaphore;

public class Regul extends Thread {
	public static final int OFF = 0;
	public static final int BEAM = 1;
	public static final int BALL = 2;
	
	private PI inner = new PI("PI");
	private PID outer = new PID("PID");

	private AnalogIn analogInAngle; 
	private AnalogIn analogInPosition; 
	private AnalogOut analogOut;

	private ReferenceGeneratorProxy referenceGenerator;
	private OpComProxy opcom;

	private int priority;
	private boolean WeShouldRun = true;
	private long starttime;
	private Semaphore mutex; // used for synchronization at shut-down

	private ModeMonitor modeMon;

	private double yAngle,yPos,y,yref,u;
	
	// Inner monitor class
	class ModeMonitor {
		private int mode;

		// Synchronized access methods
		public synchronized void setMode(int newMode) {
			mode = newMode;
		}

		public synchronized int getMode() {
			return mode;
		}
	}

	public Regul(int pri) {
		priority = pri;
		mutex = new Semaphore(1);
		try {
			analogInAngle = new AnalogIn(1);
			analogInPosition = new AnalogIn(0);
			analogOut = new AnalogOut(0);
		} catch (Exception e) { //catch (IOChannelException e) { 
			System.out.print("Error: IOChannelException: ");
			System.out.println(e.getMessage());
		}
		modeMon = new ModeMonitor();
	}

	public void setOpCom(OpComProxy opcom) {
		// Written by you
		this.opcom = opcom;
	}

	public void setRefGen(ReferenceGeneratorProxy referenceGenerator){
		// Written by you
		this.referenceGenerator=referenceGenerator;
	}

	// Called in every sample in order to send plot data to OpCom
	private void sendDataToOpCom(double yref, double y, double u) {
		double x = (double)(System.currentTimeMillis() - starttime) / 1000.0;
		DoublePoint dp = new DoublePoint(x,u);
		PlotData pd = new PlotData(x,yref,y);
		opcom.putControlDataPoint(dp);
		opcom.putMeasurementDataPoint(pd);
	}

	public synchronized void setInnerParameters(PIParameters p) {
		// Written by you
		inner.setParameters(p);
	} 

	public synchronized PIParameters getInnerParameters() {
		// Written by you
		return inner.getParameters();
	}

	public synchronized void setOuterParameters(PIDParameters p) {
		// Written by you
		outer.setParameters(p);
	}

	public synchronized PIDParameters getOuterParameters(){
		// Written by you
		return outer.getParameters();
	}

	public void setOFFMode(){
		// Written by you
		modeMon.setMode(OFF);
	}

	public void setBEAMMode(){
		// Written by you
		modeMon.setMode(BEAM);
	}

	public void setBALLMode(){
		// Written by you
		modeMon.setMode(BALL);
	}

	public int getMode(){
		// Written by you
		return modeMon.getMode();
	}

	// Called from OpCom when shutting down
	public synchronized void shutDown() {
		WeShouldRun = false;
		mutex.take();
		try {
			analogOut.set(0.0);
		} catch (Exception e) { //catch (IOChannelException x) {
		}
	}

	private double limit(double v, double min, double max) {
		if (v < min) {
			v = min;
		} else {
			if (v > max) {
				v = max;
			}
		}
		return v;
	}

	public void run() {
		long duration;
		long t = System.currentTimeMillis();
		starttime = t;

		setPriority(priority);
		mutex.take();
		while (WeShouldRun) {
			switch (modeMon.getMode()) {
			case OFF: {
				// Code for the OFF mode. 
				// Written by you.
				// Should include resetting the controllers
				// Should include a call to sendDataToOpCom  
				inner.reset();
				outer.reset();
				try{
					analogOut.set(0);
				}catch (Exception e) { //catch(IOChannelException x){
				
				}
				sendDataToOpCom(0,0,0);
				break;
			}
			case BEAM: {
				// Code for the BEAM mode
				// Written by you.
				// Should include a call to sendDataToOpCom
				try{
					y = analogInAngle.get();
				}catch (Exception e) { //catch(IOChannelException x){
				}
				yref = referenceGenerator.getRef();
				u = limit(inner.calculateOutput(y,yref),-10,10);
				System.out.println("u=" + u);
				try{
					analogOut.set(u);
				}catch (Exception e) { //catch(IOChannelException x){
				}
				inner.updateState(u);
				sendDataToOpCom(yref,y,u);
				break;
			}
			case BALL: {
				// Code for the BALL mode
				// Written by you.
				// Should include a call to sendDataToOpCom 
				try{
					yAngle = analogInAngle.get();
					yPos = analogInPosition.get();
				}catch (Exception e) { //catch(IOChannelException x){
				}
				yref = referenceGenerator.getRef();
				u = limit(inner.calculateOutput(yAngle,outer.calculateOutput(yPos,yref)),-10,10);
				try{
					analogOut.set(u);
				}catch (Exception e) { //catch(IOChannelException x){
				}
				outer.updateState(u);
				inner.updateState(u);
				sendDataToOpCom(yref,yPos,u);
				break;
			}
			default: {
				System.out.println("Error: Illegal mode.");
				break;
			}
			}
			// sleep
			t = t + inner.getHMillis();
			duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (InterruptedException x) {
				}
			}
		}
		mutex.give();
	}
}
